#include "assert.h"
#include "mmu.h"
#include "bus.h"
#include "log.h"
bool CMMU::read_byte (__u32 address,__u8 *p_dat)
{
	__u32 mva;
	__u8 dat;
	// mmu lookup tlb and permission chek
	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,0,0)) return false;
	}
	if(is_cache_enabled())
		dat=cache.read_byte(mva);
	else
		dat=bus_read_byte(mva);
		
	*p_dat=dat;
	return true ; //success
}

bool CMMU::read_short(__u32 address,__u16*p_dat)
{
	__u32 mva;
	__u16 dat;
	// mmu lookup tlb and permission chek
	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,0,0)) return false;
	}
	if(is_cache_enabled())
		dat=cache.read_short(mva);
	else
		dat=bus_read_short(mva);
		
	*p_dat=dat;
	return true ; //success
}
bool CMMU::read_int(__u32 address,__u32*p_dat)
{
	__u32 mva;
	__u32 dat;
	// mmu lookup tlb and permission chek



	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,0,0)) return false;
	}
	if(is_cache_enabled())
		dat=cache.read_int(mva);
	else
		dat=bus_read_int(mva);
		
	*p_dat=dat;
	return true ; //success
}

bool CMMU::read_insn(__u32 address,__u32*p_dat)
{
	__u32 mva;
	__u32 dat;
	// mmu lookup tlb and permission chek
	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,0,true)) return false;
	}
	if(is_cache_enabled())
		dat=cache.read_int(mva);
	else
		dat=bus_read_int(mva);
		
	*p_dat=dat;
	return true ; //success

}
bool CMMU::write_byte (__u32 address,__u8  dat)
{
	__u32 mva;
	// mmu lookup tlb and permission chek
	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,true,0)) return false;
	}
	if(is_cache_enabled())
		cache.write_byte(mva,dat);
	else
		bus_write_byte(mva,dat);
		

	return true ; //success
}
bool CMMU::write_short(__u32 address,__u16 dat)
{
	__u32 mva;
	// mmu lookup tlb and permission chek
	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,true,0)) return false;
	}
	if(is_cache_enabled())
		cache.write_short(mva,dat);
	else
		bus_write_short(mva,dat);
		
	return true ; //success
}
bool CMMU::write_int(__u32 address,__u32 dat)
{
	__u32 mva;
	//if(address<0xc0115fec+4&&address>0xc0115fec-0x40)
	//	__asm int 3;
	// mmu lookup tlb and permission chek
	if(!is_enabled())
	{
		mva=address;
	}
	else
	{
		if(!translate_and_check(address,&mva,true,0)) return false;
	}
	if(is_cache_enabled())
		cache.write_int(mva,dat);
	else
		bus_write_int(mva,dat);


	return true ; //success
}

__u32 CMMU::translate_va(__u32 va,TLB_ENTRY* p_tlb_entry)
{
	__u32 mva;
	union
	{
		__u32 level1;
		LEVEL1_FAULT l1_fault;
		LEVEL1_COARSE l1_coarse;
		LEVEL1_SECTION l1_section;
		LEVEL1_RVSD l1_resved;
		LEVEL1_COARSE_NO_SUBPG l1_coarse_no_sp;
		LEVEL1_SECTION_NO_SUBPG l1_section_no_sp;
		LEVEL1_SUPERSECTION_NO_SUBPG l1_supersection_no_sp;
	};
	union
	{
		__u32 level2;
		LEVEL2_FAULT l2_fault;
		LEVEL2_PAGE_LARGE l2_large;
		LEVEL2_PAGE_SMALL l2_small;
		LEVEL2_PAGE_SMALL_EXT l2_small_ext;
		LEVEL2_PAGE_LARGE_NO_SUBPG l2_large_no_sp;
		LEVEL2_PAGE_SMALL_EXT_NO_SUBPG l2_small_ext_no_sp;
	};
	
	if(p_tlb_entry->section)
	{

		assert ((p_tlb_entry->pte&3)==2);
		level1=p_tlb_entry->pte;
		//XP(bit[23]) Extended page table configure. This bit configures the hardware page table translation
		//mechanism:
		//0 = Subpage AP bits enabled.
		//1 = Subpage AP bits disabled. In this case, hardware translation tables support additional
		//features.
		if(!cp15.ControlRegister().XP)
		{
			mva=(l1_section.base<<20)+(va&0xfffff);
		}
		else if(!l1_section_no_sp.super)
		{
			mva=(l1_section_no_sp.base<<20)+(va&0xfffff);
		}
		else
		{
			//mva_hi=
			mva=(l1_supersection_no_sp.baselo<<24)+(va&0xffffff);
		}
	}
	else
	{
		level2=p_tlb_entry->pte;

		//XP(bit[23]) Extended page table configure. This bit configures the hardware page table translation
		//mechanism:
		//0 = Subpage AP bits enabled.
		//1 = Subpage AP bits disabled. In this case, hardware translation tables support additional
		//features.
		if(!cp15.ControlRegister().XP)
		{
	
			switch(level2&3)
			{
			case 0:
				assert(0);
				break;
			case 1:
				mva=(l2_large.base<<16)+(va&0xffff);;
				break;
			case 2:
				mva=(l2_small.base<<12)+(va&0xfff);;
				break;
			case 3:
				mva=(l2_small_ext.base<<12)+(va&0xfff);;
				break;
			};
		}
		else
		{
			switch(level2&3)
			{
			case 0:
			case 3:
				assert(0);
				break;
			case 1:
				mva=(l2_large_no_sp.base<<16)+(va&0xffff);;
				break;
			case 2:
				mva=(l2_small_ext_no_sp.base<<12)+(va&0xfff);
				break;
			}
		}
	}

	return mva;
}
bool CMMU::permission_check(TLB_ENTRY* p_tlb_entry,bool code,bool write)
{
	return true;
}
//
// hardware page table walk
//
bool CMMU::fix_tlb(__u32 va,TLB_ENTRY *p_tlb_entry)
{
	bool nofault=true;
	//
	// level 0 :choose base from ttbr0,tbr1 by ttbc.N
	//
	union
	{
		__u32 base;
		TTBase TTBR;
	};
	__u32 shift;
	__u32 offset;

	if(cp15.reg2.s.TTBaseCtrl.N==0
		||(va>>(32-cp15.reg2.s.TTBaseCtrl.N))==0)
	{
		base =cp15.reg2.w[0]&~0x3fff;
		shift=cp15.reg2.s.TTBaseCtrl.N;
	}
	else
	{
		base =cp15.reg2.w[1]&~0x3fff;
		shift=0;
	}
	base >>=shift;

	offset=va>>20;
	union
	{
		__u32 level1;
		LEVEL1_FAULT l1_fault;
		LEVEL1_COARSE l1_coarse;
		LEVEL1_SECTION l1_section;
		LEVEL1_RVSD l1_resved;
		LEVEL1_COARSE_NO_SUBPG l1_coarse_no_sp;
		LEVEL1_SECTION_NO_SUBPG l1_section_no_sp;
		LEVEL1_SUPERSECTION_NO_SUBPG l1_supersection_no_sp;
	};
	union
	{
		__u32 level2;
		LEVEL2_FAULT l2_fault;
		LEVEL2_PAGE_LARGE l2_large;
		LEVEL2_PAGE_SMALL l2_small;
		LEVEL2_PAGE_SMALL_EXT l2_small_ext;
		LEVEL2_PAGE_LARGE_NO_SUBPG l2_large_no_sp;
		LEVEL2_PAGE_SMALL_EXT_NO_SUBPG l2_small_ext_no_sp;
	};
	//
	// level 1 fetch:
	//
	level1=bus_read_int(base+4*offset); // cacheable?

	//
	// section tlb load or level 2 fetch:
	//
	switch(level1&3)
	{
	case 0:
		//If bits[1:0] == 0b00, the associated modified virtual addresses are unmapped, and attempts to access
		//them generate a translation fault

		nofault=false;
		break;
	case 2:
		if(!cp15.ControlRegister().XP)
		{
			p_tlb_entry->va32=va&~0xfffff;
			p_tlb_entry->blk_size=0xfffff+1;
		}
		else if(!l1_section_no_sp.super)
		{
			p_tlb_entry->va32=va&~0xfffff;
			p_tlb_entry->blk_size=0xfffff+1;
		}
		else
		{
			//mva_hi=
			p_tlb_entry->va32=va&=~0xffffff;
			p_tlb_entry->blk_size=0xffffff+1;
		}
		p_tlb_entry->pte=level1;
		p_tlb_entry->section=1;

		break;
	case 3:
		//If bits[1:0] == 0b11, the entry gives the physical address of a fine second-level table prior to
		//VMSAv6, and is RESERVED in VMSAv6.
		break;
	case 1:
		//
		// second level fetch
		//
		level2=bus_read_int((l1_coarse.base<<10)+4*((va>>12)&0xff));
		switch(level2&3)
		{
		case 0:
		case 3:
			nofault=false;

			break;
		case 1:
			p_tlb_entry->va32=va&~0xfffff;
			p_tlb_entry->blk_size=0xfffff+1;
			break;
		case 2:
			p_tlb_entry->va32=va&~0xfff;
			p_tlb_entry->blk_size=0xfff+1;
			break;
		}
		p_tlb_entry->pte=level2;
		p_tlb_entry->section=0;

		break;

	}
	if(va>=0x000000
		&&va<0x80000000) 
		printf("$:%x  %x\n",va,p_tlb_entry->va32);
	 return nofault;
}
bool CMMU::translate_and_check(__u32 va,__u32 *p_mva,bool write,bool code)
{
	TLB_ENTRY tlb_entry;
	__u32 mva;


			//if(va==0x0c0009ff8)
			//__asm int 3;
	if((code?itlb:dtlb).lookup(va,cp15.ContextId().ASID,&tlb_entry)!=TLB_HIT)
	{


		// translate fault
		if(!fix_tlb(va,&tlb_entry))
		{
			cp15.setFSR(0,code);
			cp15.setFAR(va,code);		
			return false;
		}

		tlb_entry.asid=cp15.ContextId().ASID;
		// update tlb


		//log("%d,add:%x %x %x\n",code,va,tlb_entry.va32,tlb_entry.pte);
		(code?itlb:dtlb).update_translation(&tlb_entry);
	}


	if(!permission_check(&tlb_entry,code,write))
	{
	// Permission Section 0b01101
	//Page 0b01111
		cp15.setFSR(tlb_entry.section?0xd:0xf,code);
		cp15.setFAR(va,code);

		return false;
	}


	*p_mva=translate_va(va,&tlb_entry);

	return true;
}